import os
import time
import RPi.GPIO as GPIO
from _XiaoRGEEK_SERVO_ import XR_Servo

GPIO.setmode(GPIO.BCM)

##################### DO NOT CHANGE! (start) #####################

MAX_FREQUENCY = 500

# LED
LED0 = 10
LED1 = 9
LED2 = 25
GPIO.setwarnings(False)
GPIO.setup(LED0, GPIO.OUT, initial=GPIO.HIGH)
GPIO.setup(LED1, GPIO.OUT, initial=GPIO.HIGH)
GPIO.setup(LED2, GPIO.OUT, initial=GPIO.HIGH)

# motors
ENA = 13
ENB = 20
IN1 = 19
IN2 = 16
IN3 = 21
IN4 = 26
GPIO.setup(ENA, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(ENB, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(IN1, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(IN2, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(IN3, GPIO.OUT, initial=GPIO.LOW)
GPIO.setup(IN4, GPIO.OUT, initial=GPIO.LOW)
p_IN1, p_IN2, p_IN3, p_IN4 = GPIO.PWM(IN1, MAX_FREQUENCY), GPIO.PWM(IN2, MAX_FREQUENCY), GPIO.PWM(IN3, MAX_FREQUENCY), GPIO.PWM(IN4, MAX_FREQUENCY)

# servo
horizontal_servo = 7
vertical_servo = 8
Servo = XR_Servo()

##################### DO NOT CHANGE! ( end ) #####################

def isNumber(s):
	try:
		float(s)
		return True
	except ValueError:
		return False

def servoSettings():
	now_servo = horizontal_servo
	while True:
		command = input('current: %s>' % ('horizontal servo' if now_servo == horizontal_servo else 'vertical servo',))
		if command == 'quit' or command == 'exit':
			break
		elif command == 'change':
			now_servo = horizontal_servo + vertical_servo - now_servo
		elif isNumber(command):
			Servo.XiaoRGEEK_SetServoAngle(now_servo, int(command))
		elif command == 'save':
			Servo.XiaoRGEEK_SaveServo()
	return None

servoSettings()